// Complimentary filter: inspired by Shane Colton's description in filter.pdf at http://web.mit.edu/first/segway/segspecs.zip

// Tweak tau to alter the performance of the complimentary filter.
// Increase tau: influence of the gyro input increases (less translation artifacts).
// Reduce tau: influence of the accelerometer input increases (faster drift-correction).
// This filtering is temporal: translations that occur over a time scale shorter than tau will be suppressed,
// and gyro changes that occur over a time scale longer than tau will have their drift corrected.
// NB: changes in the read rate of the gyro (dtMP) will change the filter's performance (change tau to compensate).

static const double DegreeToRadian = 0.0174532925;  // PI/180
static const float tau = 0.96; // unit: seconds.
float compFilter(float prevValue, int gyro, double accel, float timeStep) {
  float a = tau/(tau + dtMP);
  float b = 1 - a;
  return (float) (a*(prevValue + ((gyro*DegreeToRadian)*timeStep)) + (b*accel));
}

// Smooth - factor in 1/16th (int) = 0-16 maps to 0-100%
int smooth(int current, int previous, int factor) {
  return ((previous * (16 - factor) + (current * factor))) / 16;
}


/*
//*********************************** Kalman Filter ***************************************************8
    float Q_angle  =  0.001; //0.001
    float Q_gyro   =  0.003;  //0.003
    float R_angle  =  0.03;  //0.03

    float x_angle = 0;
    float x_bias = 0;
    float P_00 = 0, P_01 = 0, P_10 = 0, P_11 = 0;
    float dt, y, S;
    float K_0, K_1;

  float kalmanCalculate(float newAngle, float newRate,int looptime) {
    dt = float(looptime)/1000;                                    
    x_angle += dt * (newRate - x_bias);
    P_00 +=  - dt * (P_10 + P_01) + Q_angle * dt;
    P_01 +=  - dt * P_11;
    P_10 +=  - dt * P_11;
    P_11 +=  + Q_gyro * dt;
    
    y = newAngle - x_angle;
    S = P_00 + R_angle;
    K_0 = P_00 / S;
    K_1 = P_10 / S;
    
    x_angle +=  K_0 * y;
    x_bias  +=  K_1 * y;
    P_00 -= K_0 * P_00;
    P_01 -= K_0 * P_01;
    P_10 -= K_1 * P_00;
    P_11 -= K_1 * P_01;
    
    return x_angle;
  }

/*
//*********************************** Complementary Filter ***************************************************8
// newAngle = angle measured with atan2 using the accelerometer
// newRate = angle measured using the gyro
// looptime = loop time in millis()

float tau=0.075;
float a=0.0;
float x_angleC;

float Complementary(float newAngle, float newRate,int looptime) {
float dtC = float(looptime)/1000.0;
a=tau/(tau+dtC);
x_angleC= a* (x_angleC + newRate * dtC) + (1-a) * (newAngle);
return x_angleC;
}

float x_angle2C;
float Complementary2(float newAngle, float newRate,int looptime) {
float k=10;
float dtc2=float(looptime)/1000.0;


float x1 = (newAngle -   x_angle2C)*k*k;
float y1 = dtc2*x1 + y1;
float x2 = y1 + (newAngle -   x_angle2C)*2*k + newRate;
x_angle2C = dtc2*x2 + x_angle2C;

return x_angle2C;
}
*/


